﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;


namespace KinectedMusic.Kinect
{
    public enum TrackStatus { Entering, Entered, Exiting, Exited };

    public class BoundingBox
    {
        private double minDistanceFromCamera;
        private double boundsDepth;
        private double boundsWidth;

        private TrackStatus trackStatus;
        private DateTime timeSinceLastTrackStatusChange;

        private const int skeletonCount = 6;
        private Skeleton[] allSkeletons = new Skeleton[skeletonCount];

        public event EventHandler<TrackStatusArgs> RaiseTrackStatusChange;

        public BoundingBox()
        {
            this.trackStatus = TrackStatus.Exited;
            this.timeSinceLastTrackStatusChange = DateTime.Now;
            //TODO: where from
            this.MinDistanceFromCamera = 1.0;//SettingsManager.Instance.MinDistanceFromCamera;
            this.BoundsDepth = 1.0;//SettingsManager.Instance.BoundsDepth;
            this.BoundsWidth = 1.0;//SettingsManager.Instance.BoundsWidth;

        }


        public double MinDistanceFromCamera
        {
            get { return this.minDistanceFromCamera; }
            set
            {
                this.minDistanceFromCamera = value;
            }
        }

        public double BoundsWidth
        {
            get { return this.boundsWidth; }
            set
            {
                this.boundsWidth = value;
            }
        }

        public double BoundsDepth
        {
            get { return this.boundsDepth; }
            set
            {
                this.boundsDepth = value; ;
            }
        }

        public Skeleton TrackSkelton(AllFramesReadyEventArgs e, KinectSensor kinectSensor)
        {
            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {
                if (skeletonFrameData == null)
                {
                    //if (trackStatus != TrackStatus.Exited)
                    //{
                    //    ChangeTrackingStatus(TrackStatus.Exited);
                    //}
                    return null;
                }


                skeletonFrameData.CopySkeletonDataTo(allSkeletons);
                this.ChooseClosestSkeletons(allSkeletons, 1, kinectSensor);

                //TODO: TEST!!!!
                Skeleton skeleton = (from s in allSkeletons
                                  where s.TrackingState == SkeletonTrackingState.Tracked
                                  select s).FirstOrDefault();
                
                if (skeleton == null)
                {
                    //if (trackStatus != TrackStatus.Exited)
                    //{
                        ChangeTrackingStatus(TrackStatus.Exited);
                    //}
                    return null;
                }

                if (GetUserIsInRange(skeleton.Joints[JointType.HipCenter]))
                {
                    //if (trackStatus == TrackStatus.Entered)
                    //{
                    //    ;
                    //}
                    //else if (trackStatus == TrackStatus.Entering)
                    //{
                    //    if (DateTime.Now.CompareTo(timeSinceLastTrackStatusChange.AddMilliseconds(3000.0)) > 0)
                    //    {
                    //        ChangeTrackingStatus(TrackStatus.Entered);
                    //    }
                    //}
                    //else
                    //{
                    //    ChangeTrackingStatus(TrackStatus.Entering);
                    //}
                    ChangeTrackingStatus(TrackStatus.Entered);
                }
                else
                {
                    //if (trackStatus == TrackStatus.Exited)
                    //{
                    //    ;
                    //}
                    //else if (trackStatus == TrackStatus.Exiting)
                    //{
                    //    if (DateTime.Now.CompareTo(timeSinceLastTrackStatusChange.AddMilliseconds(5000.0)) > 0)
                    //    {
                    //        ChangeTrackingStatus(TrackStatus.Exited);
                    //    }
                    //}
                    //else
                    //{
                    //    ChangeTrackingStatus(TrackStatus.Exiting);
                    //}
                    ChangeTrackingStatus(TrackStatus.Exited);
                }

                return skeleton;

            }
        }

        private void ChangeTrackingStatus(TrackStatus changingTrackStatus)
        {
            trackStatus = changingTrackStatus;
            timeSinceLastTrackStatusChange = DateTime.Now;

            EventHandler<TrackStatusArgs> handler = RaiseTrackStatusChange;
            if (handler != null)
            {
                handler(this, new TrackStatusArgs((TrackStatus)changingTrackStatus));
            }
        }

        private void ChooseClosestSkeletons(IEnumerable<Skeleton> skeletonDataValue, int count, KinectSensor kinectSensor)
        {
            SortedList<float, int> depthSorted = new SortedList<float, int>();

            foreach (Skeleton s in skeletonDataValue)
            {
                if (s.TrackingState != SkeletonTrackingState.NotTracked)
                {
                    // TODO: Think of the offset what it should be!! .. test the ABS!!
                    float valueX = s.Position.Z;// + SettingsManager.Instance.KinectFindSkeletonOffset);
                    while (depthSorted.ContainsKey(valueX))
                    {
                        valueX += 0.0001f;
                    }

                    depthSorted.Add(valueX, s.TrackingId);
                }
            }
           
            this.ChooseSkeletonsFromList(depthSorted.Values, count, kinectSensor);
        }


        private void ChooseSkeletonsFromList(IList<int> list, int max, KinectSensor kinectSensor)
        {
            if (kinectSensor.SkeletonStream.IsEnabled)
            {
                int argCount = Math.Min(list.Count, max);

                if (argCount == 0)
                {
                    kinectSensor.SkeletonStream.ChooseSkeletons();
                }

                if (argCount == 1)
                {
                    kinectSensor.SkeletonStream.ChooseSkeletons(list[0]);
                }

                if (argCount >= 2)
                {
                    kinectSensor.SkeletonStream.ChooseSkeletons(list[0], list[1]);
                }
            }
        }

        private bool GetUserIsInRange(Joint joint)
        {
            return joint.Position.Z > this.MinDistanceFromCamera &
                joint.Position.Z < (this.MinDistanceFromCamera + this.BoundsDepth)
                & joint.Position.X > -this.BoundsWidth / 2 &
                joint.Position.X < this.BoundsWidth / 2;
        }
    }
}
